#!/usr/bin/python3
from launch import LaunchDescription
from launch_ros.actions import Node, LifecycleNode
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
import os

def generate_launch_description():
    # 获取雷达参数文件路径
    lslidar_dir = get_package_share_directory('lslidar_driver')
    lidar_param_file = os.path.join(lslidar_dir, 'params', 'lidar_uart_ros2', 'lsn10.yaml')
    
    # 获取MoveIt demo启动文件路径
    moveit_dir = get_package_share_directory('chaojixiaoche_moveit')
    demo_launch_file = os.path.join(moveit_dir, 'launch', 'demo.launch.py')
    
    # 加载MoveIt配置（用于arm_control）
    moveit_config = MoveItConfigsBuilder("chaojikeaidexiaoche", package_name="chaojixiaoche_moveit").to_moveit_configs()
    
    # N10串口雷达节点
    lidar_node = LifecycleNode(
        package='lslidar_driver',
        executable='lslidar_driver_node',
        name='lslidar_driver_node',
        output='screen',
        emulate_tty=True,
        namespace='',
        parameters=[lidar_param_file]
    )
    
    # 引入MoveIt demo启动文件
    demo_include = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(demo_launch_file)
    )
    
    # 使用TimerAction延迟2秒启动MoveIt demo（等待雷达先启动）
    delayed_demo = TimerAction(
        period=2.0,
        actions=[demo_include]
    )
    
    # arm_control action服务器节点
    arm_control_node = Node(
        package='arm_control',
        executable='arm_control_node',
        name='arm_control_server',
        output='screen',
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
            moveit_config.planning_pipelines,
        ],
    )
    
    # 轨迹发布节点
    trajectory_publisher_node = Node(
        package='arm_control',
        executable='trajectory_publisher_node',
        name='trajectory_publisher',
        output='screen',
        parameters=[
            {'publish_interval_ms': 150},  # 每150ms发布一个轨迹点（增加间隔让下位机有更多处理时间）
            {'point_skip_interval': 2}     # 每2个点发布一个（减少转发点数量）
        ],
    )
    
    # 使用TimerAction延迟5秒启动arm_control相关节点
    delayed_arm_control = TimerAction(
        period=5.0,  # 延迟5秒
        actions=[arm_control_node, trajectory_publisher_node]
    )
    
    # 返回LaunchDescription
    return LaunchDescription([
        lidar_node,
        delayed_demo,  # 延迟2秒启动MoveIt demo（包含RViz和joint_state_publisher_gui）
        delayed_arm_control,  # 延迟5秒启动arm_control相关节点
    ])

